require("sys")
local dcmd = {}
local shift = 77
local speedZero=69
-- 是否是返回
local  backSeedState=false 
local function CmdMain()
    local function onSpeed(speed)
        if speed == nil then
            return
        end
        -- （71-91） 前进  停止是69  后退是67到50
        local factSpeed = speedZero
        if speed > 0 then
            backSeedState=false
            factSpeed=math.floor(speed/5)+71
        end
        if speed < 0 then
            factSpeed=67+math.floor(speed/5.8)
            if not backSeedState then
                backSeedState=true
                pwm.open(10, 50, factSpeed, 0, 1000)
                pwm.open(1, 50, factSpeed, 0, 1000)
                sys.wait(10)
                pwm.open(10, 50, speedZero, 0, 1000)
                pwm.open(1, 50, speedZero, 0, 1000)
                sys.wait(30)
                pwm.open(10, 50, factSpeed, 0, 1000)
                pwm.open(1, 50, factSpeed, 0, 1000)
                sys.wait(30)
                pwm.open(10, 50, speedZero, 0, 1000)
                pwm.open(1, 50, speedZero, 0, 1000)
                sys.wait(30)
            end
        end
        pwm.open(10, 50, factSpeed, 0, 1000)
        pwm.open(1, 50, factSpeed, 0, 1000)
        log.info("set speed to : ", speed)
    end
    local function onAngle(angle)
        if angle == nil then
            return
        end
        angle = angle + shift
        log.info("set angle to : ", angle)
        pwm.open(14, 50, angle, 0, 1000)
    end
    local function onCmd(dat)
        local function doCtrl(speed, angle)
            onSpeed(dat.dat.speed)
            onAngle(dat.dat.angle)
        end
        local function onNet(dat)
            if (dat.src == "dnet") then
                log.info("------网络命令来了------", dat.dat.speed, dat.dat.angle)
                doCtrl(dat.dat.speed, dat.dat.angle)
            end
        end
        local function onCom(dat)
            if (dat.src == "dcom") then
                log.info("------串口命令来了------", dat.dat.speed, dat.dat.angle)
                doCtrl(dat.dat.speed, dat.dat.angle)
            end
        end
        if (dat.evt == "COMMAND") then
            onNet(dat)
            onCom(dat)
        end
    end

    pwm.open(1, 500, 0, 0, 100)
    pwm.open(10, 500, 0, 0, 100)
    pwm.open(14, 50, shift, 0, 1000)
    while true do
        local ret, dat = sys.waitUntil("ddn_event")
        if (ret == true) then
            onCmd(dat)
        end
    end
end

local function Init()
    log.info("-------dcmd init----------")
    sys.taskInit(CmdMain)
end
dcmd.Init = Init
return dcmd
